#include "depthClouddataprocessor.h"
#include "PointCloudGenerator.h"
#include <QDebug>
#include <QImage>
#include <QThread>
#include <QFile>
#include <QHash>
#include <QVariant>
#include <QCoreApplication>
#include <QDateTime>
#include <QtConcurrent>
#include <yaml-cpp/yaml.h>  


void DepthCloudDataProcessor::parseDepthDataUint16(const QByteArray& data, QImage& image, int width, int height) {
    const unsigned short* rawData = reinterpret_cast<const unsigned short*>(data.constData());
    int dataSize = data.size() / 2; // uint16是2字节

    // 计算最大深度值用于归一化
    unsigned short maxDepth = 0;
    for (int i = 0; i < qMin(width * height, dataSize); i++) {
        if (rawData[i] > maxDepth) {
            maxDepth = rawData[i];
        }
    }

    // 直接访问图像内存，避免setPixel的性能开销
    for (int y = 0; y < height; y++) {
        uchar* line = image.scanLine(y);
        for (int x = 0; x < width; x++) {
            int index = y * width + x;
            if (index < dataSize) {
                unsigned short depth = rawData[index];
                // 将深度值映射到RGB颜色（0=黑色，255=白色）
                int gray = static_cast<int>(depth * 255.0 / maxDepth);
                line[x * 3] = gray;     // R
                line[x * 3 + 1] = gray; // G
                line[x * 3 + 2] = gray; // B
            }
        }
    }
}



DepthCloudDataProcessor::DepthCloudDataProcessor(QObject* parent)
    : QObject(parent)
{
    QString exeDir = QCoreApplication::applicationDirPath();
    QString calibPath = exeDir + "/mtx_dist.txt";
    std::string calib_file = calibPath.toStdString();
    generator = new PointCloudGenerator(calib_file);
    //initGenerator();
}

// 定义一个函数来解析 YAML 文件中的键值对
QHash<QString, QVariant> DepthCloudDataProcessor::parseYaml(const QString& filePath) {
    QHash<QString, QVariant> result;
    QFile file(filePath);

    if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
        qDebug() << "无法打开文件";
        return result;
    }

    QTextStream in(&file);
    QString currentSection;
    while (!in.atEnd()) {
        QString line = in.readLine().trimmed();
        if (line.isEmpty() || line.startsWith('#')) {
            continue; // 跳过空行和注释
        }

        if (line.endsWith(':')) {
            // 这是一个节（section）的开始
            currentSection = line.left(line.length() - 1);
        }
        else if (line.contains(':')) {
            // 这是一个键值对
            QStringList parts = line.split(':');
            QString key = parts[0].trimmed();
            QString value = parts[1].trimmed();

            // 处理值的类型转换
            if (value.startsWith('"') && value.endsWith('"')) {
                // 字符串类型
                value = value.mid(1, value.length() - 2);
                result[key] = value;
            }
            else if (value == "true") {
                // 布尔类型（true）
                result[key] = true;
            }
            else if (value == "false") {
                // 布尔类型（false）
                result[key] = false;
            }
            else {
                // 尝试转换为整数或浮点数
                bool ok;
                int intValue = value.toInt(&ok);
                if (ok) {
                    result[key] = intValue;
                }
                else {
                    double doubleValue = value.toDouble(&ok);
                    if (ok) {
                        result[key] = doubleValue;
                    }
                }
            }
        }
    }
    file.close();
    return result;
}


void DepthCloudDataProcessor::processDepthFrame(QSharedPointer<QByteArray> data)
{
#if 1
    qDebug() << "DepthDataProcessor::processDepthFrame" << QThread::currentThreadId();
    QByteArray temByteArr = *data;

    m_receiveBuffer.append(temByteArr.toHex().toUpper());
    while (true)
    {
        int endIndex = m_receiveBuffer.indexOf("EEFFEEFF");
        if (endIndex == -1)
        {
            // 没有找到帧尾退出循环
            break;
        }
        int frameLength = endIndex + 8;
        if (m_receiveBuffer.size() >= frameLength)
        {
            // 提取完整的一帧
            QByteArray completeFrame = m_receiveBuffer.left(frameLength);
            // 提取剩余数据
            QByteArray remainingData = m_receiveBuffer.mid(frameLength);
            m_frameCounter++;
            if (m_frameCounter >= FRAME_COUNTER_MAX)
            {
                m_frameCounter = 0;
                qDebug() << "帧计数器已达到阈值，重置为0";
            }
            // 只处理偶数帧，丢弃奇数帧
            if (m_frameCounter % 2 == 0)
            {
                processCompleteFrame(completeFrame);
            }
            else {
                qDebug() << "丢弃奇数帧" << m_frameCounter;
            }
            m_receiveBuffer = remainingData;
        }
        else {
            // 数据不完整，退出循环
            break;
        }
    }
#endif
}

/**
 * @brief 将十六进制毫秒时间戳转换为可读时间字符串
 * @param hexMsec 十六进制毫秒时间戳（如 "668344B597010000"）
 * @return 格式化后的时间字符串（yyyy-MM-dd hh:mm:ss.zzz）
 */
QString DepthCloudDataProcessor::convertHexMsecToDateTime(const QString& hexMsec) {
    // 检查输入有效性
    if (hexMsec.length() % 2 != 0) {
        return "Invalid hex length, must be even";
    }

    // 1. 倒序十六进制字符串（按字节倒序）
    QByteArray byteArray = QByteArray::fromHex(hexMsec.toUtf8());
    std::reverse(byteArray.begin(), byteArray.end());
    QString reversedHex = QString(byteArray.toHex(' ').replace(" ", ""));
    qDebug() << "Reversed hex:" << reversedHex;

    // 2. 转换为毫秒数值
    bool ok = false;
    quint64 msecValue = reversedHex.toULongLong(&ok, 16);
    if (!ok) {
        return "Failed to convert hex to number";
    }
    qDebug() << "Milliseconds:" << msecValue;

    // 3. 转换为秒（毫秒 ÷ 1000）
    quint64 secValue = msecValue / 1000;
    quint64 msecPart = msecValue % 1000; // 提取毫秒部分

    // 4. 转换为QDateTime
    QDateTime datetime = QDateTime::fromSecsSinceEpoch(secValue);
    if (!datetime.isValid()) {
        return "Invalid timestamp value";
    }

    // 5. 格式化输出（包含毫秒部分）
    return datetime.toString("yyyy-MM-dd hh:mm:ss.") +
        QString::number(msecPart, 10).rightJustified(3, '0');
}

void DepthCloudDataProcessor::initGenerator()
{
    QString exeDir = QCoreApplication::applicationDirPath();
    QString yamlPath = exeDir + "/all_node.yaml";
    try
    {
        // 从/th_itof节点读取参数
        int middle_height_start = 0;
        int middle_height_end = 0;
        int middle_width_start = 0;
        int middle_width_end = 0;
        double downsample_resolution = 0;
        YAML::Node config = YAML::LoadFile(yamlPath.toStdString());

        if (config["/th_itof"] && config["/th_itof"]["ros__parameters"])
        {
            YAML::Node params = config["/th_itof"]["ros__parameters"];
            if (params["dp_start_row"]) middle_height_start = params["dp_start_row"].as<int>();
            if (params["dp_end_row"]) middle_height_end = params["dp_end_row"].as<int>();
            if (params["dp_start_col"]) middle_width_start = params["dp_start_col"].as<int>();
            if (params["dp_end_col"]) middle_width_end = params["dp_end_col"].as<int>();
            if (params["downsample_resolution"]) downsample_resolution = params["downsample_resolution"].as<double>();

            qDebug() << "从YAML读取的点云区域参数:";
            qDebug() << "dp_start_row:" << middle_height_start;
            qDebug() << "dp_end_row:" << middle_height_end;
            qDebug() << "dp_start_col:" << middle_width_start;
            qDebug() << "dp_end_col:" << middle_width_end;
        }
        else {
            qWarning() << "未找到/th_itof节点，使用默认参数";
            // 设置默认值
            middle_height_start = 425;
            middle_height_end = 575;
            middle_width_start = 0;
            middle_width_end = 1280;
        }
        // 计算中间区域尺寸
        int middle_height = middle_height_end - middle_height_start;
        int middle_width = middle_width_end - middle_width_start;
        // 验证计算结果
        if (middle_height <= 0 || middle_width <= 0) {
            qCritical() << "计算的中间区域尺寸无效: height=" << middle_height
                << ", width=" << middle_width;
            // 设置安全默认值
            middle_height = 150;
            middle_width = 1280;
            middle_height_start = 425;
            middle_height_end = 575;
        }

        // 配置PointCloudGenerator
        generator->middle_height_start_ = middle_height_start;
        generator->middle_height_end_ = middle_height_end;
        generator->middle_height_ = middle_height;
        generator->middle_width_start_ = middle_width_start;
        generator->middle_width_end_ = middle_width_end;
        generator->middle_width_ = middle_width;
        generator->cloud->resize(generator->middle_width_ * generator->middle_height_);

        // 初始化参数默认值
        double tx = 0.0, ty = 0.0, tz = 0.0;
        double yaw = 0.0, pitch = 0.0, roll = 0.0;
        double min_x = 0.0, max_x = 5.0, min_y = -5.0, max_y = 5.0, min_z = -3.0, max_z = 3.0;
        if (config["/point_detect"] && config["/point_detect"]["ros__parameters"]) {
            YAML::Node params = config["/point_detect"]["ros__parameters"];
            // 读取平移参数
            if (params["translation_x"]) tx = params["translation_x"].as<double>();
            if (params["translation_y"]) tx = params["translation_y"].as<double>();
            if (params["translation_z"]) tx = params["translation_z"].as<double>();

            // 去读旋转参数
            if (params["yaw"]) yaw = params["yaw"].as<double>();
            if (params["pitch"]) pitch = params["pitch"].as<double>();
            if (params["roll"]) roll = params["roll"].as<double>();
            // 读取坐标范围
            if (params["min_x"]) min_x = params["min_x"].as<double>();
            if (params["max_x"]) max_x = params["max_x"].as<double>();
            if (params["min_y"]) min_y = params["min_y"].as<double>();
            if (params["max_y"]) max_y = params["max_y"].as<double>();
            if (params["min_z"]) min_z = params["min_z"].as<double>();
            if (params["max_z"]) max_z = params["max_z"].as<double>();
            // 输出读取的参数（调试用）
            qDebug() << "从YAML读取的参数:";
            qDebug() << "tx:" << tx << "ty:" << ty << "tz:" << tz;
            qDebug() << "yaw:" << yaw << "pitch:" << pitch << "roll:" << roll;
            qDebug() << "min_x:" << min_x << "max_x:" << max_x;
            qDebug() << "min_y:" << min_y << "max_y:" << max_y;
            qDebug() << "min_z:" << min_z << "max_z:" << max_z;

            // 调用setparams方法
            generator->setparams(tx, ty, tz, yaw, pitch, roll,
                min_x, max_x, min_y, max_y, min_z, max_z);
            generator->downsample_resolution = downsample_resolution;
        }
        else {
            qWarning() << "未找到/point_detect节点，使用默认参数";
        }

    }
    catch (const YAML::Exception& e) {
        qCritical() << "解析YAML参数失败:" << QString::fromStdString(e.what());
        // 使用默认参数
        generator->setparams(0.0, 0.0, 0.0, 0.00, 0.0, 0.0,
            0.0, 5.0, -5.0, 5.0, -3.0, 3.0);
    }
}

void DepthCloudDataProcessor::processCompleteFrame(const QByteArray& data)
{
    QElapsedTimer timer;
    timer.start();

    QByteArray dataCopy = data;
    qDebug() << "DepthDataProcessor::processDepthFrame" << QThread::currentThreadId();
    //qDebug() << "222" << m_receiveBuffer.mid(40, 22);
    //qDebug() << "333" << m_receiveBuffer.mid(42, 22);
    //qDebug() << m_receiveBuffer.mid(0, 64);
    //qDebug() << m_receiveBuffer.mid(64, 2);
    //qDebug() << m_receiveBuffer.mid(66, 8);
    quint32 depthWidth = convertHexStringToDecimal(dataCopy.mid(66, 8));
    //qDebug() << m_receiveBuffer.mid(74, 8);
    quint32 depthHeight = convertHexStringToDecimal(dataCopy.mid(74, 8));

    // 01000000 代表quint8   02000000代表quint16
    //qDebug() << "11" << m_receiveBuffer.mid(82, 8);
    // 02000000 代表内存连续
    //qDebug() << "22" << m_receiveBuffer.mid(90, 8);
    // 代表数据长度
    //qDebug() << "33" << m_receiveBuffer.mid(98, 16);
    quint32 dataLength = convertHexStringToDecimal(dataCopy.mid(98, 16));

    qDebug() << "head:" << dataCopy.mid(0, 114);
    qDebug() << "data" << dataCopy.mid(42, 16);
    QString hexMsec = dataCopy.mid(42, 16);
    QString timeMsec = convertHexMsecToDateTime(hexMsec);
    qDebug() << "Decimal value:" << timeMsec;
    // 将十进制值转换为时间（假设为Unix时间戳，单位：秒）

    dataCopy.remove(0, 114);
    QByteArray newByteArray;
    if (data.endsWith("EEFFEEFF"))
    {
        newByteArray = dataCopy.left(dataCopy.length() - 32);
    }
    QByteArray str = hexStringToLittleEndian(newByteArray);
    qDebug() << "str" << str.size();

    if (str.size() == depthWidth * depthHeight * 2)
    {
        cv::Mat depthMat = hexStringToDepthMat(newByteArray, depthWidth, depthHeight);
        if (!depthMat.empty())
        {
            // 这里可能会存在 yaml的内容和上来的实际数据不一样的问题
            if (generator->middle_width_ == depthWidth && generator->middle_height_ == depthHeight)
            {
                generator->generate(depthMat);
                emit pointCloudReady(generator->final_filtered_cloud_);
            }
            
        }
    }
    newByteArray.clear();
    //m_receiveBuffer.clear();
    str.clear();
    dataCopy.clear();
    // 获取耗时（毫秒）
    qint64 elapsedMs = timer.elapsed();
    qDebug() << "操作耗时:" << elapsedMs << "毫秒";
}

cv::Mat DepthCloudDataProcessor::hexStringToDepthMat(const QString& hexString, int width, int height)
{
    // 计算像素总数和期望的十六进制字符串长度
    int pixelCount = width * height;
    int expectedHexLength = pixelCount * 4; // 每个quint16需要4个十六进制字符

    // 快速验证输入长度
    if (hexString.length() != expectedHexLength) {
        qDebug() << "十六进制字符串长度错误! 期望:" << expectedHexLength << " 实际:" << hexString.length();
        return cv::Mat();
    }

    // 转换十六进制字符串为字节数组（每个quint16对应2字节）
    QByteArray byteArray = QByteArray::fromHex(hexString.toUtf8());
    if (byteArray.size() != pixelCount * 2) {
        qDebug() << "字节数组转换失败，长度不匹配。期望:" << pixelCount * 2 << " 实际:" << byteArray.size();
        return cv::Mat();
    }

    // 创建深度图矩阵（CV_16UC1表示16位无符号单通道）
    cv::Mat depthMat(height, width, CV_16UC1);
    if (depthMat.empty()) {
        qDebug() << "深度图矩阵创建失败";
        return cv::Mat();
    }

    // 转换字节数组为uchar指针，便于直接访问
    const uchar* hexData = reinterpret_cast<const uchar*>(byteArray.constData());

    // 创建像素索引列表（预分配内存提高性能）
    QVector<int> pixelIndices;
    pixelIndices.reserve(pixelCount);
    for (int i = 0; i < pixelCount; ++i) {
        pixelIndices.append(i);
    }

    // 使用阻塞式并行计算（直接返回结果）
    QVector<quint16> depthValues = QtConcurrent::blockingMapped(pixelIndices, [=](int pixelIndex) {
        // 计算字节偏移（小端序处理：低字节在前，高字节在后）
        int byteOffset = pixelIndex * 2;
        quint8 lowByte = hexData[byteOffset];
        quint8 highByte = hexData[byteOffset + 1];
        return (quint16)(highByte << 8 | lowByte);
        });

    // 将并行计算结果复制到cv::Mat中
    if (depthValues.size() == pixelCount) {
        memcpy(depthMat.data, depthValues.constData(), pixelCount * sizeof(quint16));
    }
    else {
        qDebug() << "并行计算结果长度不匹配，期望:" << pixelCount << " 实际:" << depthValues.size();
        return cv::Mat();
    }

    return depthMat;

}

void DepthCloudDataProcessor::onRecvInitGeneral()
{
    initGenerator();
}

quint32 DepthCloudDataProcessor::convertHexStringToDecimal(const QString& hexStr)
{
    QByteArray byteArray = QByteArray::fromHex(hexStr.toUtf8());
    QByteArray reversedBytes = byteArray;
    std::reverse(reversedBytes.begin(), reversedBytes.end());

    quint32 result = 0;
    for (char byte : reversedBytes) {
        result = (result << 8) | static_cast<quint8>(byte);
    }
    return result;
}

QByteArray DepthCloudDataProcessor::hexStringToLittleEndian(const QString& hexString)
{
    QByteArray byteArray = QByteArray::fromHex(hexString.toUtf8());
    std::reverse(byteArray.begin(), byteArray.end());
    return byteArray;
}